#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import socket
import threading
import logging

# 配置日志
logger = logging.getLogger(__name__)

class SocketHandler:
    def __init__(self, socket_host='0.0.0.0', socket_port=8888, command_callback=None):
        """
        初始化Socket处理器
        :param socket_host: Socket监听地址
        :param socket_port: Socket监听端口
        :param command_callback: 收到命令时的回调函数，格式为callback(vx, vy, vw)
        """
        self.socket_host = socket_host
        self.socket_port = socket_port
        self.command_callback = command_callback
        
        self.socket_server = None
        self.client_socket = None
        self.client_address = None
        
        self.running = False
        self.client_thread = None
    
    def setup(self):
        """
        设置Socket服务器
        """
        try:
            self.socket_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.socket_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
            self.socket_server.bind((self.socket_host, self.socket_port))
            self.socket_server.listen(1)
            logger.info(f"Socket服务器已启动，监听 {self.socket_host}:{self.socket_port}")
            return True
        except Exception as e:
            logger.error(f"无法设置Socket服务器: {e}")
            return False
    
    def start(self):
        """
        启动Socket处理器
        """
        if not self.setup():
            return False
        
        self.running = True
        self.client_thread = threading.Thread(target=self._handle_clients)
        self.client_thread.daemon = True
        self.client_thread.start()
        
        return True
    
    def _handle_clients(self):
        """
        处理客户端连接和数据
        """
        while self.running:
            try:
                # 等待客户端连接
                logger.info("等待客户端连接...")
                self.client_socket, self.client_address = self.socket_server.accept()
                logger.info(f"客户端已连接: {self.client_address}")
                
                # 处理客户端数据
                self._handle_client_data()
            except Exception as e:
                if self.running:  # 只在非主动关闭的情况下记录错误
                    logger.error(f"处理客户端连接时出错: {e}")
                    import time
                    time.sleep(1)  # 避免在错误情况下循环过快
    
    def _handle_client_data(self):
        """
        处理单个客户端的数据
        """
        while self.running and self.client_socket:
            try:
                data = self.client_socket.recv(1024).decode('utf-8').strip()
                if not data:
                    logger.info("客户端断开连接")
                    self.client_socket = None
                    break
                
                logger.info(f"接收到控制命令: {data}")
                
                # 验证数据格式 (vx,vy,vw)
                parts = data.split(',')
                if len(parts) == 3:
                    try:
                        vx, vy, vw = map(float, parts)
                        
                        # 调用回调函数处理命令
                        if self.command_callback:
                            self.command_callback(vx, vy, vw)
                    except ValueError:
                        logger.error(f"无效的控制命令格式: {data}")
                else:
                    logger.error(f"无效的控制命令: {data}，应为'vx,vy,vw'格式")
            except Exception as e:
                if self.running:  # 只在非主动关闭的情况下记录错误
                    logger.error(f"处理客户端数据时出错: {e}")
                self.client_socket = None
                break
    
    def send_data(self, data):
        """
        发送数据到客户端
        :param data: 要发送的数据
        :return: 是否成功发送
        """
        if not self.client_socket:
            return False
        
        try:
            self.client_socket.sendall(data.encode('utf-8'))
            return True
        except Exception as e:
            logger.error(f"发送数据到客户端失败: {e}")
            self.client_socket = None
            return False
    
    def stop(self):
        """
        停止Socket处理器
        """
        self.running = False
        
        # 关闭客户端连接
        if self.client_socket:
            try:
                self.client_socket.close()
            except:
                pass
            self.client_socket = None
        
        # 关闭Socket服务器
        if self.socket_server:
            try:
                self.socket_server.close()
            except:
                pass
            self.socket_server = None
        
        logger.info("Socket处理器已停止")
